
module SMACCMPilot.Flight.Law.TestApp where

import Ivory.Tower

import SMACCMPilot.Flight.Platform
import SMACCMPilot.Flight.Datalink
import SMACCMPilot.Flight.Datalink.UART
import SMACCMPilot.Flight.Datalink.ControllableVehicle
import SMACCMPilot.Flight.IO
import SMACCMPilot.Flight.Law

import           SMACCMPilot.Comm.Tower.Attr
import           SMACCMPilot.Comm.Tower.Interface.ControllableVehicle

app :: (e -> FlightPlatform)
    -> Tower e ()
app tofp = do

  cvapi@(attrs, _streams) <- controllableVehicleAPI

  fp  <- fmap tofp getEnv
  mon <- datalinkTower tofp cvapi
    (uartDatalink (fp_clockconfig . tofp) (fp_telem fp) 115200)
  monitor "uart_dma" mon

  -- Don't hook anything up to outputs!
  (_, output_cl) <- channel
  (_, output_motors) <- channel

  -- Inputs generated by flightIOTower will be forwarded:
  rcinput_ui <- channel
  rcinput_cm <- channel
  rcinput_am <- channel

  flightIOTower tofp attrs
    (fst rcinput_ui)
    (fst rcinput_cm)
    (fst rcinput_am)
    output_cl
    output_motors

  -- No sensors for the unit test:
  sensors_output <- channel

  let lawInputs = LawInputs
        { lawinput_rcinput_arming   = snd rcinput_am
        , lawinput_rcinput_ui       = snd rcinput_ui
        , lawinput_rcinput_modes    = snd rcinput_cm
        , lawinput_telem_arming     = attrReaderChan (armingRequest attrs)
        , lawinput_telem_ui         = attrReaderChan (userInputRequest attrs)
        , lawinput_telem_modes      = attrReaderChan (controlModesRequest attrs)
        , lawinput_px4io_state      = attrReaderChan (px4ioState attrs)
        , lawinput_sensors_output   = snd sensors_output
        }


  control_law <- channel
  user_input_result <- channel
  arming_status <- channel
  lawTower lawInputs (fst arming_status) (fst control_law) (fst user_input_result)

  attrProxy (armingStatus attrs) (snd arming_status)
  attrProxy (controlLaw attrs) (snd control_law)
  attrProxy (userInput attrs) (snd user_input_result)

